#pragma once

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <nav_msgs/msg/odometry.hpp>

#include <tf2_ros/transform_broadcaster.h>
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"

#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>

#include <array>

#include "hardware_interface/hardware_interface.h"
#include "robot_msgs/msg/hardware.hpp"



// ===================== 协方差配置 =====================
namespace robot_covariance_config
{
    inline constexpr std::array<double, 36> odom_pose_covariance = {
        1e-3, 0,    0,    0,    0,    0,
        0,    1e-3, 0,    0,    0,    0,
        0,    0,    1e6,  0,    0,    0,
        0,    0,    0,    1e6,  0,    0,
        0,    0,    0,    0,    1e6,  0,
        0,    0,    0,    0,    0,    1e3
    };
}

//负责 ROS 的发布/订阅逻辑、定时器调度等
class RosInterface : public rclcpp::Node {
    public:
        RosInterface();
        ~RosInterface();
       
        void InitParameters();
        void InitPublishers();
        void InitSubscribers();
        void InitTimers();
        void InitHardware();

        void CmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg); 
        void CmdUtilsCallback(const robot_msgs::msg::Hardware::SharedPtr msg); 

        void GetSerialDataPoll();
        void SendVelocityCommandPoll();
        void OdomPubPoll(); 
        void HardwareStatePubPoll();

        EncoderCounts GetEncoderCountsFromHardware();
        HardwareStates GetHardwareStatesFromHardware();

    private:
        std::string __port = "/dev/ttyUSB0";
        int __baudrate = 115200;
     

        double __linear = 0.0; // 线速度 m/s
        double __angular = 0.0; // 角速度 rad/s
        double __track_width = 0.440; // 轮距，单位m

        double __x = 0.0, __y = 0.0, __theta = 0.0; // 位姿信息
        
        EncoderCounts __last_encoder_counts;
        HardwareStates __hardware_states;
        
        rclcpp::Time __last_speed_time;
        rclcpp::Time __now_speed_time;

        bool __is_pub_tf = true;
        
        std::string __frame_id = "odom";
        std::string __child_frame_id = "base_link";
     
       
        rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr __cmd_vel_sub;
        rclcpp::Subscription<robot_msgs::msg::Hardware>::SharedPtr __cmd_utils_sub;

        std::shared_ptr<HardwareInterface> __hardware_interface;    //硬件接口
        
        int __get_serial_data_poll_interval_ms = 50;
        rclcpp::TimerBase::SharedPtr __get_serial_data_poll_timer;
        
        int __send_velocity_command_poll_interval_ms = 50;
        rclcpp::TimerBase::SharedPtr __send_velocity_command_timer;
        
        std::shared_ptr<tf2_ros::TransformBroadcaster> __tf_broadcaster;
        rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr __odom_pub;
        rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr __odom_pos_pub;
        int __odom_pub_poll_interval_ms = 50;
        rclcpp::TimerBase::SharedPtr __odom_pub_poll_timer;

        rclcpp::Publisher<robot_msgs::msg::Hardware>::SharedPtr __hardware_state_pub;
        int __hardware_state_pub_poll_interval_ms = 100;
        rclcpp::TimerBase::SharedPtr __hardware_state_pub_poll_timer;
};



